#include "DepthSensor.h"
#define DevAddr 0x01 //设备ID
static unsigned char _SendData[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
static unsigned char _RecvData[7] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

short DepthSensor::ReadFun(DepthSensor::FuncCode func)
{
    _SendData[0] = (DevAddr & 0xFF);//设备地址
    _SendData[1] = (DepthSensor::ReadFuncCode & 0xFF); //读取
    _SendData[2] = ((func>>8) & 0xFF);//待读取的起始地址高八位
    _SendData[3] = (func & 0xFF);//待读取的起始地址高八位
    _SendData[4] = (0x00); //读取的数量
    _SendData[5] = 0x01; //读取的数量低八位
    uint16_t crc = this->modbus->CRC16_MudBus(_SendData, 6);//进行CRC校验
    _SendData[6] = (crc & 0xFF);//CRC的低八位
    _SendData[7] = ((crc>>8) & 0xFF);//CRC的高八位
    SendData(_SendData, 8);//发送数据
    delay(200);//等待100ms返回数据
    RecvData(_RecvData, 7);//接收数据
    uint16_t crc_Recv = this->modbus->CRC16_MudBus(_RecvData, 5);//进行CRC校验
    if (((crc_Recv>>8) & 0xFF) != _RecvData[6] || (crc_Recv & 0xFF) != _RecvData[5])//校验成功
    {
        Serial.println("ReadFun : CRC Check failed!!!");
        return 0x0000;
    }
    return ((_RecvData[3]<<8) | _RecvData[4]);
}

void DepthSensor::SetFunc(DepthSensor::FuncCode func, short SetData)
{
    _SendData[0] = (DevAddr & 0xFF);//设备地址
    _SendData[1] = (DepthSensor::SetFuncCOde & 0xFF); //读取
    _SendData[2] = ((func>>8) & 0xFF);//待读取的起始地址高八位
    _SendData[3] = (func & 0xFF);
    _SendData[4] = ((SetData>>8) & 0xFF);
    _SendData[5] = (SetData & 0xFF);
    uint16_t crc = this->modbus->CRC16_MudBus(_SendData, 6);//进行CRC校验
    _SendData[6] = (crc & 0xFF);//CRC的低八位
    _SendData[7] = ((crc>>8) & 0xFF);//CRC的高八位
    SendData(_SendData, 8);//发送数据
    delay(200);//等待100ms返回数据
    RecvData(_SendData, 8);//接收数据
    uint16_t crc_Recv = this->modbus->CRC16_MudBus(_SendData, 6);//进行CRC校验
    if (((crc_Recv>>8) & 0xFF) != _SendData[7] || (crc_Recv & 0xFF) != _SendData[6])//校验成功
    {
        Serial.println("SetFunc : RecvData CRC Check failed!!!");
        return;
    }
    return;
}

int DepthSensor::ReadDepth()
{
    _SendData[0] = (DevAddr & 0xFF);
    _SendData[1] = (DepthSensor::ReadFuncCode & 0xFF);//读取
    _SendData[2] = ((DepthSensor::ReadMeasurementsAddr >> 8) & 0xFF);
    _SendData[3] = (DepthSensor::ReadMeasurementsAddr & 0xFF);
    _SendData[4] = 0x00; //读取的数量
    _SendData[5] = 0x01; //读取的数量低八位
    _SendData[6] = (CRC_for_ReadDepth & 0xFF);
    _SendData[7] = ((CRC_for_ReadDepth>>8) & 0xFF);
    // char Data[64];
    // sprintf(Data,"%X %X %X %X %X %X %X %X",_SendData[0],_SendData[1],_SendData[2],_SendData[3],_SendData[4],_SendData[5],_SendData[6],_SendData[7]);
    // Serial.println(Data);
    SendData(_SendData, 8);//发送数据
    delay(100);//等待100ms返回数据
    RecvData(_RecvData, 7);//接收数据
    //为了简化，不在校验
    return (((_RecvData[3]<<8) | _RecvData[4])); // 此块应该乘以10
}

void DepthSensor::SetBauteRate(DepthSensor::_BaudRate baudRate)
{
    SetFunc(DepthSensor::DevBaudRateAddr, baudRate);
}

void DepthSensor::SetDevNum(unsigned short DevId)
{
    SetFunc(DepthSensor::DevNumAddr, DevId);
}

void DepthSensor::initFunc(Modbus_RTU * _modbus)
{
    this->modbus = _modbus;
    _SendData[0] = (DevAddr & 0xFF);
    _SendData[1] = (DepthSensor::ReadFuncCode & 0xFF);//读取
    _SendData[2] = ((DepthSensor::ReadMeasurementsAddr >> 8) & 0xFF);
    _SendData[3] = (DepthSensor::ReadMeasurementsAddr & 0xFF);
    _SendData[4] = 0x00; //读取的数量
    _SendData[5] = 0x01; //读取的数量低八位
    CRC_for_ReadDepth = this->modbus->CRC16_MudBus(_SendData, 6);//进行CRC校验
}

void DepthSensor::SendData(unsigned char * pdata, int num)
{
    this->modbus->SendData(pdata, num);
}

bool DepthSensor::RecvData(unsigned char * poutData, int num)
{
    return this->modbus->RecvData(poutData, num);
}